#include "useless_cmd.h"
#include "log/zuclog.h"
#include "motion/mot_priv.h"
#include "motion/motion_debug.h"

#define MOTCMD_INFO(fmt, ...) zuclog_info("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_ERR(fmt, ...) zuclog_error("MOTCMD", fmt, ##__VA_ARGS__)
#define MOTCMD_DEBUG(fmt, ...) zuclog_debug("MOTCMD", fmt, ##__VA_ARGS__)

int CmdSetJointMinFerror::process()
{
    MOTCMD_DEBUG("SET_JOINT_MIN_FERROR");
    MOTCMD_DEBUG(" %d", joint_num_);
    zucmot_config_change();
    zucmot_joint_t* joint = &(joints[joint_num_]);
    if (joint == 0 || minFerror_ < 0.0)
    {
        return 0;
    }
    joint->min_ferror = minFerror_;
    return 0;
}

int CmdJointUnhome::process()
{
    MOTCMD_DEBUG("JOINT_UNHOME %d", joint_num_);
    if (joint_num_ == -2)
    {  // -1 means home all
        int jj;
        zucmot_joint_t* syncjoint;
        for (jj = 0; jj < zucmotConfig->numJoints; jj++)
        {
            syncjoint = &joints[jj];
            SET_JOINT_HOMED_FLAG(syncjoint, 0);
        }
        return 0;
    }

    zucmot_joint_t* joint = &(joints[joint_num_]);
    if (joint == NULL)
    {
        return 0;
    }
    SET_JOINT_HOMED_FLAG(joint, 0);
    return 0;
}

int CmdJointHome::process()
{
    MOTCMD_DEBUG("JOINT_HOME %d", joint_num_);
    if (joint_num_ == -1)
    {  // -1 means home all
        int jj;
        zucmot_joint_t* syncjoint;
        for (jj = 0; jj < zucmotConfig->numJoints; jj++)
        {
            syncjoint = &joints[jj];
            SET_JOINT_HOMED_FLAG(syncjoint, 1);
        }
        return 0;
    }

    zucmot_joint_t* joint = &(joints[joint_num_]);
    if (joint == NULL)
    {
        return 0;
    }
    SET_JOINT_HOMED_FLAG(joint, 1);
    return 0;
}

int CmdMotorsServoOn::process()
{
    MOTCMD_INFO("CmdMotorsServoOn");
    /*make sure the free_tp stay still*/
    // for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     joints[joint_num].free_tp.enable = 0;
    //     joints[joint_num].free_tp.curr_vel = 0.0;
    // }
    // zucmot_hal_data->motors_servo_on = 1;
    return 0;
}

int CmdMotorsServoOff::process()
{
    MOTCMD_INFO("CmdMotorsServoOff");
    zucmot_hal_data->motors_servo_on = 0;
    return 0;
}

int CmdJointActivate::process()
{
    MOTCMD_INFO("ZUCMOT_JOINT_ACTIVATE %d\n", joint_num_);
    zucmot_joint_t* joint = &(joints[joint_num_]);
    if (joint == 0)
    {
        return 0;
    }
    SET_JOINT_ACTIVE_FLAG(joint, 1);
    return 0;
}

int CmdJointDeactivate::process()
{
    MOTCMD_INFO("ZUCMOT_JOINT_DEACTIVATE %d\n", joint_num_);
    zucmot_joint_t* joint = &(joints[joint_num_]);
    if (joint == 0)
    {
        return 0;
    }
    SET_JOINT_ACTIVE_FLAG(joint, 0);
    return 0;
}

int CmdSetJointHomingParams::process()
{
    MOTCMD_INFO("SET_JOINT_HOMING_PARAMS %d: %f", joint_num_, offset_);
    zucmot_config_change();
    zucmot_joint_t* joint = &(joints[joint_num_]);
    if (joint == 0)
    {
        return 0;
    }
    joint->home_offset = offset_;
    joint->home = home_;
    joint->home_final_vel = home_final_vel_;
    joint->home_search_vel = search_vel_;
    joint->home_latch_vel = latch_vel_;
    joint->home_flags = homingflag_;
    joint->home_sequence = home_sequence_;
    joint->volatile_home = volatile_home_;
    return 0;
}

int CmdUpdateJointHomingParams::process()
{
    MOTCMD_DEBUG("UPDATE_JOINT_HOMING_PARAMS");
    MOTCMD_DEBUG(" %d", joint_num_);
    zucmot_config_change();
    zucmot_joint_t* joint = &(joints[joint_num_]);
    if (joint == 0)
    {
        return 0;
    }
    joint->home_offset = offset_;
    joint->home = home_;
    return 0;
}

int CmdOverrideLimits::process()
{
    /* this command can be issued with joint < 0 to re-enable
			 limits, but they are automatically re-enabled at the
			 end of the next jog */

    MOTCMD_DEBUG("OVERRIDE_LIMITS");
    MOTCMD_DEBUG(" %d", joint_num_);
    if (joint_num_ < 0)
    {
        /* don't override limits */
        MOTCMD_DEBUG("override off");
        zucmotStatus->overrideLimitMask = 0;
    }
    else
    {
        MOTCMD_DEBUG("override on");
        zucmotStatus->overrideLimitMask = 0;
        for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
        {
            /* point at joint data */
            zucmot_joint_t* joint = &joints[joint_num];
            /* only override limits that are currently tripped */
            if (GET_JOINT_NHL_FLAG(joint))
            {
                zucmotStatus->overrideLimitMask |= (1 << (joint_num * 2));
            }
            if (GET_JOINT_PHL_FLAG(joint))
            {
                zucmotStatus->overrideLimitMask |= (2 << (joint_num * 2));
            }
        }
    }

    zucmotDebug->overriding = 0;

    for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    {
        /* point at joint data */
        zucmot_joint_t* joint = &joints[joint_num];
        /* clear joint errors */
        SET_JOINT_ERROR_FLAG(joint, 0);
    }
    return 0;
}

int CmdSetAxisPositionLimits::process()
{
    /* set the position limits for axis */
    /* can be done at any time */
    MOTCMD_INFO("SET_AXIS_POSITION_LIMITS axis: %d %f %f\n", axis_num_, minLimit_, maxLimit_);
    zucmot_config_change();
    zucmot_axis_t* axis = &axes[axis_num_];
    if (axis == 0)
    {
        return 0;
    }
    axis->min_pos_limit = minLimit_;
    axis->max_pos_limit = maxLimit_;
    return 0;
}

int CmdSetAxisLockingJoint::process()
{
    MOTCMD_DEBUG("SET_AXIS_ACC_LOCKING_JOINT axis:%d", axis_num_);
    zucmot_config_change();
    zucmot_axis_t* axis = &axes[axis_num_];
    if (axis == 0)
    {
        return 0;
    }
    axis->locking_joint = joint_num_;
    return 0;
}

int CmdSetAxisVelLimit::process()
{
    /* set the max axis vel */
    /* can be done at any time */
    MOTCMD_INFO("SET_AXIS_VEL_LIMITS axis: %d %f\n", axis_num_, vel_);
    zucmot_config_change();
    zucmot_axis_t* axis = &axes[axis_num_];
    if (axis == 0)
    {
        return 0;
    }
    axis->vel_limit = vel_;
    return 0;
}

int CmdSetAxisAccLimit::process()
{
    /* set the max axis acc */
    /* can be done at any time */
    MOTCMD_INFO("ZUCMOT_SET_AXIS_ACC_LIMIT axis:%d %f\n", axis_num_, acc_);
    zucmot_config_change();
    zucmot_axis_t* axis = &axes[axis_num_];
    if (axis == 0)
    {
        return 0;
    }
    axis->acc_limit = acc_;
    return 0;
}

int CmdSetMotorRatedcurr::process()
{
    MOTCMD_INFO("ZUCMOT_SET_MOTOR_RATEDCURR :");
    // for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     motors[joint_num].rated_curr = motorParam_[joint_num];
    //     MOTCMD_INFO("rated_curr[%d] = %f\n", joint_num, motors[joint_num].rated_curr);
    // }
    return 0;
}

int CmdSetMotorRatedtorq::process()
{
    MOTCMD_INFO("ZUCMOT_SET_MOTOR_RATEDTORQ:");
    // for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     motors[joint_num].rated_torq = motorParam_[joint_num];
    //     MOTCMD_INFO("rated_torq[%d] = %f\n", joint_num, motors[joint_num].rated_torq);
    // }
    return 0;
}

int CmdSetMotorRatedtransratio::process()
{
    MOTCMD_INFO("ZUCMOT_SET_MOTOR_RATEDTRANSRATIO: ");
    // for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++)
    // {
    //     motors[joint_num].rated_trans_ratio = motorParam_[joint_num];
    //     MOTCMD_INFO("rated_trans_ratio[%d] = %f\n", joint_num, motors[joint_num].rated_trans_ratio);
    // }
    return 0;
}

int CmdSetRobotModel::process()
{
    // int robotSeries = robot_model_ / 10000;
    // zucmotConfig->robotModel = robot_model_;
    // MOTCMD_INFO("ZUCMOT_SET_ROBOT_MODEL = %d\n", zucmotConfig->robotModel);
    // setKineRobotSeries(robotSeries);
    // setRobotModel(robotSeries);
    // zucmot_config_change();
    return 0;
}

int CmdSetJointOffsetAndDir::process()
{
    MOTCMD_INFO("ZUCMOT_SET_JOINT_OFFSET_AND_DIR\n");
    // int32_t offset[ZUCMOT_MAX_JOINTS] = {};
    // double offset_kine[ZUCMOT_MAX_JOINTS] = {0.0};
    // for (int i = 0; i < ZUCMOT_MAX_JOINTS; i++)
    // {
    //     offset_kine[i] = offset_[i];
    //     offset[i] = (int32_t)(offset_[i] * DEG2ABSCNT);
    //     zucmotStatus->joint_offset[i] = offset[i];
    //     zucmotStatus->joint_offset_deg[i] = offset_kine[i];
    //     zucmotStatus->joint_dir[i] = dir_[i];
    // }

    // memcpy(zucmotStatus->joint_offset_fix, offset_kine, sizeof(double) * ZUCMOT_MAX_JOINTS);
    // set_robot_series_joint_cfg(zucmotConfig->robotModel / 10000, offset, dir_);
    // set_joint_offset_fix(offset_kine);
    // zucmot_config_change();
    return 0;
}

int CmdSetNumJoints::process()
{
    MOTCMD_DEBUG("SET_NUM_JOINTS, %d", joint_num_);
    // zucmotConfig->numJoints = joint_num_;
    // recheck_set_joints_num(zucmotConfig->numJoints);
    //    // tpManager_setjointnum(zucmotConfig->numJoints);
    // set_servo_joint_num(zucmotConfig->numJoints);
    // zucmot_config_change();
    return 0;
}

int CmdSetDhParam::process()
{
    MOTCMD_INFO("ZUCMOT_SET_DH_PARAM\n");
    // zucmotStatus->dhParam = dhParam_;
    // MOTCMD_INFO("DH_PARAM : d = %f %f %f %f %f %f %f\n",
    //             zucmotStatus->dhParam.d[0],
    //             zucmotStatus->dhParam.d[1],
    //             zucmotStatus->dhParam.d[2],
    //             zucmotStatus->dhParam.d[3],
    //             zucmotStatus->dhParam.d[4],
    //             zucmotStatus->dhParam.d[5],
    //             zucmotStatus->dhParam.d[6]);
    // MOTCMD_INFO("DH_PARAM : a = %f %f %f %f %f %f %f\n",
    //             zucmotStatus->dhParam.a[0],
    //             zucmotStatus->dhParam.a[1],
    //             zucmotStatus->dhParam.a[2],
    //             zucmotStatus->dhParam.a[3],
    //             zucmotStatus->dhParam.a[4],
    //             zucmotStatus->dhParam.a[5],
    //             zucmotStatus->dhParam.a[6]);
    // MOTCMD_INFO("DH_PARAM joint_homeoff = %f %f %f %f %f %f %f\n",
    //             zucmotStatus->dhParam.joint_homeoff[0],
    //             zucmotStatus->dhParam.joint_homeoff[1],
    //             zucmotStatus->dhParam.joint_homeoff[2],
    //             zucmotStatus->dhParam.joint_homeoff[3],
    //             zucmotStatus->dhParam.joint_homeoff[4],
    //             zucmotStatus->dhParam.joint_homeoff[5],
    //             zucmotStatus->dhParam.joint_homeoff[6]);
    // MOTCMD_INFO("DH_PARAM alpha = %f %f %f %f %f %f %f\n",
    //             zucmotStatus->dhParam.alpha[0],
    //             zucmotStatus->dhParam.alpha[1],
    //             zucmotStatus->dhParam.alpha[2],
    //             zucmotStatus->dhParam.alpha[3],
    //             zucmotStatus->dhParam.alpha[4],
    //             zucmotStatus->dhParam.alpha[5],
    //             zucmotStatus->dhParam.alpha[6]);

    // updateKineDHParams(zucmotStatus->dhParam);
    // zucmot_update_comp_coefs();
    return 0;
}

int CmdSetDynParam::process()
{
    MOTCMD_DEBUG("ZUCMOT_SET_DYN_PARAM");
    // zucmotConfig->dynParam = dynParam_;
    // for (int i = 0; i < zucmotConfig->numJoints * 14; i++) { MOTCMD_INFO("kineticParam[%d] %lf\n", i, zucmotConfig->dynParam.kineticK[i]); }
    // zucmot_update_comp_coefs();
    // zucmot_config_change();
    return 0;
}

int CmdSetJointAccLimit::process()
{
    /* set joint max acceleration */
    /* can do it at any time */
    MOTCMD_INFO("SET_JOINT_ACC_LIMIT %d: %f", joint_num_, acc_);
    // zucmot_config_change();
    // /* check joint range */
    // zucmot_joint_t* joint = &(joints[joint_num_]);
    // if (joint == 0)
    // {
    //     return 0;
    // }
    // joint->acc_limit = acc_;
    return 0;
}

int CmdSetInitPose::process()
{
    // 开机时加载usersettings文件里的，切换机型时加载机型文件里的
    // MOTCMD_INFO("ZUCMOT_SET_INIT_POSE\n");
    // double oripos[ZUCMOT_MAX_JOINTS];
    // int cmpsame = 0;
    // for (int joint_num_ = 0; joint_num_ < zucmotConfig->numJoints; joint_num_++)
    // {
    //     if (zucmotConfig->safetyCfg.init_traj_jpos[joint_num_] == trajInitPosj_[joint_num_])
    //         cmpsame++;
    //     oripos[joint_num_] = zucmotConfig->safetyCfg.init_traj_jpos[joint_num_];
    //     zucmotConfig->safetyCfg.init_traj_jpos[joint_num_] = trajInitPosj_[joint_num_];
    //     MOTCMD_INFO("init pos %d: %f\n", joint_num_, zucmotConfig->safetyCfg.init_traj_jpos[joint_num_]);
    // }
    // unsigned int ori_crc = zucmotStatus->safetyCRC;
    // zucmotStatus->safetyCRC = crc32((const UINT8*)&zucmotConfig->safetyCfg, sizeof(SafetyConfig));
    // // @FIXME liwang 此处checksum信息未考虑7轴
    // if (cmpsame != 6 && task_init_finish_flag == 1)
    // {
    //     char str[MAX_ERROR_MSG_STR_LEN];
    //     snprintf(str,
    //              MAX_ERROR_MSG_STR_LEN,
    //              "{\"fixed_key\":[\"%#x\", \"%#x\",\"%.3f;%.3f;%.3f;%.3f;%.3f;%.3f\",\"%.3f;%.3f;%.3f;%.3f;%.3f;%.3f\"]}",
    //              ori_crc,
    //              zucmotStatus->safetyCRC,
    //              oripos[0],
    //              oripos[1],
    //              oripos[2],
    //              oripos[3],
    //              oripos[4],
    //              oripos[5],
    //              zucmotConfig->safetyCfg.init_traj_jpos[0],
    //              zucmotConfig->safetyCfg.init_traj_jpos[1],
    //              zucmotConfig->safetyCfg.init_traj_jpos[2],
    //              zucmotConfig->safetyCfg.init_traj_jpos[3],
    //              zucmotConfig->safetyCfg.init_traj_jpos[4],
    //              zucmotConfig->safetyCfg.init_traj_jpos[5]);
    //     reportInfo(INFO_SAFETY_CONF_INIT_POSE_CHANGED, "checksum changed # %s ", str);
    // }
    return 0;
}

int CmdSetInitPoseTol::process()
{
    // double orig_tol;
    // orig_tol = zucmotConfig->safetyCfg.init_jpos_tol;
    // zucmotConfig->safetyCfg.init_jpos_tol = tol_;
    // MOTCMD_INFO("ZUCMOT_SET_INIT_POSE_TOL: %f\n", zucmotConfig->safetyCfg.init_jpos_tol);
    // unsigned int ori_crc = zucmotStatus->safetyCRC;
    // zucmotStatus->safetyCRC = crc32((const UINT8*)&zucmotConfig->safetyCfg, sizeof(SafetyConfig));
    // if (orig_tol != zucmotConfig->safetyCfg.init_jpos_tol && task_init_finish_flag == 1)
    // {
    //     char str[MAX_ERROR_MSG_STR_LEN];
    //     snprintf(str,
    //              MAX_ERROR_MSG_STR_LEN,
    //              "{\"fixed_key\":[\"%#x\", \"%#x\", \"%.3f\",\"%.3f\"]}",
    //              ori_crc,
    //              zucmotStatus->safetyCRC,
    //              orig_tol,
    //              zucmotConfig->safetyCfg.init_jpos_tol);
    //     reportInfo(INFO_SAFETY_CONF_INIT_POSE_TOL_CHANGED, "checksum changed # %s ", str);
    // }
    return 0;
}

int CmdSetRobotBaseOffset::process()
{
    // zucmotConfig->appAngleX = trajsetbaseoffset_.appAngleX;
    // zucmotConfig->appAngleZ = trajsetbaseoffset_.appAngleZ;
    // zucmotConfig->base_offset = trajsetbaseoffset_.base_offset;

    // // gravity 方向与appAngleX, appAngleZ 为同一物理量
    // zucmotStatus->gravity_rpy[0] = zucmotConfig->appAngleX / 180.0 * M_PI;
    // zucmotStatus->gravity_rpy[2] = zucmotConfig->appAngleZ / 180.0 * M_PI;

    // int jntNum = 0;
    // double jointPos[ZUCMOT_MAX_JOINTS] = {0};
    // for (jntNum = 0; jntNum < zucmotConfig->numJoints; jntNum++) { jointPos[jntNum] = joints[jntNum].pos_cmd; }

    // axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine()->fkine(
    //     RobJointVal(jointPos), zucmotStatus->carte_pos_cmd, &zucmotConfig->tool_offset, &zucmotConfig->user_offset, &zucmotConfig->base_offset);

    // /*keep joint position as current, update the cartesian position*/
    // ZucPose finalUserOffset = getFinalUserOffset();
    // axisgroup::AxisGroupMgr::instance().get_axisgroup(group_id_)->kine()->fkine(
    //     RobJointVal(jointPos), zucmotStatus->carte_pos_desired, &zucmotConfig->tool_offset, &finalUserOffset, &zucmotConfig->base_offset);

    // //    tpManager_tpSetGoalPose(zucmotStatus->carte_pos_desired);
    // //    tpManager_tpSetPose(zucmotStatus->carte_pos_desired);

    // MOTCMD_INFO("Install Angle = %f %f\n", zucmotConfig->appAngleX, zucmotConfig->appAngleZ);
    // MOTCMD_INFO("Install rpy = %f %f %f\n", zucmotConfig->base_offset.r, zucmotConfig->base_offset.p, zucmotConfig->base_offset.y);

    // // zucmot_update_comp_coefs();
    // // updateTeleopInfo();
    // zucmot_config_change();
    return 0;
}

int CmdSetVel::process()
{
    /* set the velocity for subsequent moves */
    /* can do it at any time */
    MOTCMD_INFO("ZUCMOT_SET_VEL: %f, %lf", vel_, ini_maxvel_);
    // zucmotStatus->vel = vel_;  // 无用变量
    //    // tpManager_tpSetVMax(zucmotStatus->vel, ini_maxvel_);
    return 0;
}

int CmdSetVelLimit::process()
{
    MOTCMD_INFO("ZUCMOT_SET_VEL_LIMIT: %f\n", vel_);
    // zucmot_config_change();
    // /* set the absolute max velocity for all subsequent moves */
    // /* can do it at any time */
    // zucmotConfig->limitVel = vel_;
    //    // tpManager_tpSetVlimit(zucmotConfig->limitVel);
    // zucmot_config_change();
    return 0;
}

int CmdSetAcc::process()
{
    /* set the max acceleration */
    /* can do it at any time */
    MOTCMD_INFO("ZUCMOT_SET_ACC: %f\n", acc_);
    // zucmotStatus->acc = acc_;
    //    // tpManager_tpSetAmax(zucmotStatus->acc);
    return 0;
}

int CmdSetMaxJointJerk::process() { return 0; }

int CmdSetMaxCarteJerk::process() { return 0; }